﻿using System;
using System.Threading;
using LBNHelpers;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;
using NXTNetMF;

namespace FEZDomino_HelloWorld
{
	public class Program
	{
		public static void Main()
		{
			//I2CManager.HardwareBus.ScanI2CDevices();

			var servo1 = new ServoDriver((Cpu.Pin) FEZ_Pin.PWM.Di5);
			var servo2 = new ServoDriver((Cpu.Pin) FEZ_Pin.PWM.Di6);
			var servo3 = new ServoDriver((Cpu.Pin) FEZ_Pin.PWM.Di8);
//			var servo4 = new ServoDriver((Cpu.Pin) FEZ_Pin.PWM.Di5);
			//for (int i = 0; i < 180; i+=20)
			//{

			servo3.setRange(600, 2400);

			servo3.Degree = 0;
			Thread.Sleep(2000);

			servo3.Degree = 90;
			Thread.Sleep(2000);

			servo3.Degree = 180;
			Thread.Sleep(2000);

			servo3.Degree = 90;
			Thread.Sleep(2000);

			servo1.Degree = servo2.Degree = servo3.Degree = 0;
			Thread.Sleep(5000);

			return;

			servo1.Degree = servo2.Degree = servo3.Degree = 180;
			Thread.Sleep(5000);
			//}


			return;

			using (var softbus = I2CManager.CreateSoftwareBus((Cpu.Pin)FEZ_Pin.Digital.Di1, (Cpu.Pin)FEZ_Pin.Digital.Di0, 100))
			{
				//softbus.ScanI2CDevices();

				var mmx1 = new NxtMMX(I2CManager.HardwareBus, 0x03);
				var mmx2 = new NxtMMX(I2CManager.HardwareBus, 0x04);
				var mmx3 = new NxtMMX(softbus, 0x03);
				var pspnx = new PSPNx3(softbus, 0x01);

				mmx1.MotorsReset();
				mmx2.MotorsReset();
				mmx3.MotorsReset();
				//return;

				try
				{
					bool quit = false;
					while (quit == false)
					{
						pspnx.UpdateButtonState();
						if (pspnx.Cross == true)
						{
							quit = true;
						}

						mmx1.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, pspnx.LeftJoystick.X);
						mmx1.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, pspnx.LeftJoystick.Y);
						mmx2.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, pspnx.RightJoystick.X);
						mmx2.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, pspnx.RightJoystick.Y);

						if (pspnx.A) mmx3.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.5f);
						else if (pspnx.B) mmx3.RunUnlimited(WhichMotor.Motor1, MotorDirection.Reverse, 0.5f);
						else mmx3.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.0f);

						if (pspnx.C) mmx3.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.5f);
						else if (pspnx.D) mmx3.RunUnlimited(WhichMotor.Motor2, MotorDirection.Reverse, 0.5f);
						else mmx3.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.0f);

						Thread.Sleep(100);
					}
				}
				catch (Exception ex)
				{
					Debug.Print(ex.ToString());
				}

				mmx1.MotorsReset();
				mmx2.MotorsReset();
				mmx3.MotorsReset();

				mmx1.Dispose();
				mmx2.Dispose();
				mmx3.Dispose();
				pspnx.Dispose();

			}

			for (int i = 0; i < 10; i++)
			{
				Debug.Print("Bye bye!!!");
			}

			//mmx1.MotorsReset();
			//mmx2.MotorsReset();


			//mmx1.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.2f);
			//Thread.Sleep(500);
			//mmx1.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.3f);
			//Thread.Sleep(500);
			//mmx2.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.4f);
			//Thread.Sleep(500);
			//mmx2.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.5f);

			//Thread.Sleep(1000);

			//mmx1.MotorsReset();
			//mmx2.MotorsReset();

			//Thread.Sleep(1000);

			//mmx1.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.2f);
			//mmx1.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.3f);
			//mmx2.RunUnlimited(WhichMotor.Motor1, MotorDirection.Forward, 0.4f);
			//mmx2.RunUnlimited(WhichMotor.Motor2, MotorDirection.Forward, 0.5f);

			//Thread.Sleep(2000);

			//mmx1.MotorsReset();
			//mmx2.MotorsReset();

		}

		private static void ScanI2CPorts()
		{
			//for (ushort i = 1; i < 128; i++)
			//{
			//    var s = I2CManager.Execute(i, new I2CDevice.I2CTransaction[1] { I2CDevice.CreateReadTransaction(new byte[1]) });

			//    if (s == 1)
			//    {
			//        Debug.Print("found at: " + i.ToString());
			//    }
			//}
		}


	}
}
